﻿using System;
using System.Collections;
using System.Diagnostics;

namespace Modbus.Messages
{
	internal static class DataUtility
	{
		internal static byte[] ReadData(ModbusReader reader)
		{
			Debug.Assert(reader != null, "reader != null");

			int byteCount = (int)(reader.ReadByte());
			return reader.ReadBytes(byteCount);
		}

		internal static void WriteData(ModbusWriter writer, byte[] data)
		{
			Debug.Assert(writer != null, "writer != null");
			Debug.Assert(data != null, "data != null");
			Debug.Assert(data.Length > 0, "data.Length > 0");
			Debug.Assert(data.Length <= Byte.MaxValue, "data.Length <= byte.MaxValue");

			writer.Write((byte)(data.Length));
			writer.Write(data);
		}

		internal static byte[] EncodeDiscretes(params bool[] discretes)
		{
			Debug.Assert(discretes != null, "discretes != null");
			Debug.Assert(discretes.Length > 0, "discretes.Length > 0");

			BitArray bitArray = new BitArray(discretes);
			
			byte[] data = new byte[(bitArray.Count + 7) / 8];
			bitArray.CopyTo(data, 0);

			return data;
		}
		
		internal static bool[] DecodeDiscretes(int count, params byte[] data)
		{
			Debug.Assert(count > 0, "count > 0");
			Debug.Assert(data != null, "data != null");
			Debug.Assert(data.Length > 0, "data.Length > 0");
			Debug.Assert(count <= (8 * data.Length), "count <= (8 * data.Length)");

			BitArray bitArray = new BitArray(data);
			bitArray.Length = count;

			bool[] discretes = new bool[count];
			bitArray.CopyTo(discretes, 0);

			return discretes;
		}

		internal static byte[] EncodeRegisters(params short[] registers)
		{
			Debug.Assert(registers != null, "registers != null");
			Debug.Assert(registers.Length > 0, "registers.Length > 0");

			byte[] data = new byte[2 * registers.Length];

			for (int i = 0; i < registers.Length; i++)
			{
				data[2 * i] = (byte)(registers[i]);
				data[2 * i + 1] = (byte)(registers[i] >> 8);
			}

			return data;
		}

		internal static short[] DecodeRegisters(params byte[] data)
		{
			Debug.Assert(data != null, "data != null");
			Debug.Assert(data.Length > 0, "data.Length > 0");
			Debug.Assert((data.Length % 2) == 0, "(data.Length % 2) == 0");

			short[] registers = new short[data.Length / 2];

			for (int i = 0; i < data.Length; i += 2)
			{
				registers[i / 2] = (short)((data[i + 1] << 8) | data[i]);
			}

			return registers;
		}
	}
}
